/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package control;

import Jama.Matrix;
import java.util.ArrayList;
import java.util.List;
import world.Point;

/**
 *
 * @author default
 * Class PDController is responsible for managing movements for a bot.
 * For now, it will be used in conjuction with the reactive bot.
 * 
 */
public class PDController
{
    public PDController()
    {
        
    }
    public static List<TankAction> move(double angle, Point a, Point b, long DT)
    {
        return move(angle, a, b, DT, .04);
    }
    
    public static List<TankAction> reverse(double angle, Point a, Point b, long DT, double minError)
    {
        
        Point n = Utility.minus(b, a);
        b = Utility.minus(a, n);

        double targangle = Math.atan2(b.getY() - a.getY(), b.getX() - a.getX());

        if (targangle < 0)
        {
            targangle = 2 * Math.PI + targangle;
        }
       
        double diff = targangle - angle;

        double dist = Utility.distance(a, b);


        if (diff < 0)
        {
            diff += Math.PI * 2;
        }


        //System.out.println("target: "+radianToDegrees(targangle)+" tank: "+radianToDegrees(angle)+" diff: "+radianToDegrees(diff));
        double err = 0.0;
        double angvel = 0.0;
        double speed = 0.0;
        if (diff <= Math.PI)
        {
            //angvel = diff / Math.PI;//*diff;
            err = diff;
            
        } else
        {
            //angvel = -1 * (((Math.PI * 2) - diff) / Math.PI);//*(((Math.PI * 2) - diff));
            err = Math.PI*2 - diff;
        }
        
        
        double dT = DT/1000.0;
        
        double Kp = 1.0;
        double Kd = 1.6;
        err = err/Math.PI;
        
        double Kps = 1.0;
        double Kds = .80;
        
        double res = Kp * err + Kd * err/dT;
        
        //System.out.println("Result: "+res+" error: "+err+" dT: "+dT);
        if(dist > 80.0)
        {
            speed = 1.0;
        }
        else
        {
            speed = (Kps * (dist/80.0) + Kds * (dist/80.0)/dT) * (1-err);
            //speed = (dist/60.0)*(.5/err);
           // System.out.println(speed+" "+(dist/80.0)+" "+(1-err));
        }
        
        if(diff <= Math.PI)
        {
            angvel = res;
        }
        else
        {
            angvel = -1*res;
        }

        List<TankAction> actions = new ArrayList();
        //if(err < .04)
        //{
            actions.add(new Shoot());
        //}
        actions.add(new Speed(speed*-1, dist));
       
        actions.add(new Angvel(angvel,err*Math.PI));
        //if(err < .04)
       // {
           // actions.add(new Shoot());
       // }
        
        return actions;
    }
    
    private static boolean onLine(Point p, double slope, double b)
    {
        if (slope == Double.POSITIVE_INFINITY || slope == Double.NEGATIVE_INFINITY)
        {
            double result = Math.abs(p.getX() - b);

            return (result < .05);
        }
        double result = p.getX() * slope + b;
        result = Math.abs(result - p.getY());

        return (result < .05);
    }
    
    // moves 'bot' along the search path
    public static List<TankAction> move(double angle, Point a, Point b, long DT, double minError)
    {

        double targangle = Math.atan2(b.getY() - a.getY(), b.getX() - a.getX());

        if (targangle < 0)
        {
            targangle = 2 * Math.PI + targangle;
        }
       
        double diff = targangle - angle;

        double dist = Utility.distance(a, b);


        if (diff < 0)
        {
            diff += Math.PI * 2;
        }


        //System.out.println("target: "+radianToDegrees(targangle)+" tank: "+radianToDegrees(angle)+" diff: "+radianToDegrees(diff));
        double err = 0.0;
        double angvel = 0.0;
        double speed = 0.0;
        if (diff <= Math.PI)
        {
            //angvel = diff / Math.PI;//*diff;
            err = diff;
            
        } else
        {
            //angvel = -1 * (((Math.PI * 2) - diff) / Math.PI);//*(((Math.PI * 2) - diff));
            err = Math.PI*2 - diff;
        }
        
        
        double dT = DT/1000.0;
        
        double Kp = 1.0;
        double Kd = 1.6;
        err = err/Math.PI;
        
        double Kps = 1.0;
        double Kds = .80;
        
        double res = Kp * err + Kd * err/dT;
        
        //System.out.println("Result: "+res+" error: "+err+" dT: "+dT);
        if(dist > 80.0)
        {
            speed = 1.0;
        }
        else
        {
            speed = (Kps * (dist/80.0) + Kds * (dist/80.0)/dT) * (1-err);
            //speed = (dist/60.0)*(.5/err);
           // System.out.println(speed+" "+(dist/80.0)+" "+(1-err));
        }
        
        if(diff <= Math.PI)
        {
            angvel = res;
        }
        else
        {
            angvel = -1*res;
        }

        List<TankAction> actions = new ArrayList();
        //if(err < .04)
        //{
            actions.add(new Shoot());
        //}
        actions.add(new Speed(speed, dist));
       
        actions.add(new Angvel(angvel,err*Math.PI));
        //if(err < .04)
       // {
           // actions.add(new Shoot());
       // }
        
        return actions;
    }
    // method for movement based on Xt and Xt+1
    // This method's purpose is to incorporate acceleration into the pdcontroller (it must have a velocity of 'newVel')
    public List<TankAction> move(Point currVel, Point newVel, double DT)
    {
        double currAngle = Utility.getAngleRadians(new Point(0,0), currVel);
        double nextAngle = Utility.getAngleRadians(new Point(0,0), newVel);
        double speed=Math.sqrt(newVel.getX() * newVel.getX() + newVel.getY()*newVel.getY());
        double diff = nextAngle - currAngle;

        if (diff < 0)
        {
            diff += Math.PI * 2;
        }
        double err = 0.0;
        double angvel = 0.0;   
        if (diff <= Math.PI)
        {
            err = diff;   
        } else
        {
            err = Math.PI*2 - diff;
        }
        double dT = DT/1000.0;
        double Kp = 1.0;
        double Kd = 1.6;
        err = err/Math.PI;
        double res = Kp * err + Kd * err/dT;
        if(diff <= Math.PI)
        {
            angvel = res;
        }
        else
        {
            angvel = -1*res;
        }
        List<TankAction> actions = new ArrayList();
        actions.add(new Speed(speed, 0));
        actions.add(new Angvel(angvel,0));
        return actions;
    }
}
